import sensor, image, time
from machine import UART

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)  # 设置图像色彩格式为RGB565格式
sensor.set_framesize(sensor.QQVGA)   # 设置图像大小为160 * 120
sensor.set_auto_whitebal(True)       # 设置自动白平衡
sensor.set_brightness(3000)          # 设置亮度为3000
sensor.skip_frames(time=20)          # 跳过帧
clock = time.clock()
# 初始化串口
uart = UART(3, 115200)


R_THRESHOLDS = [
    (15, 75, 20, 90, 0, 60),
    # (10, 90, 15, 90, -30, 70),  # 主阈值范围
    # (5, 95, 10, 95, -40, 80)    # 更宽松的备用阈值范围
    # #(49, 54, 6, 62, -3, 127)
    ###   # (15, 100, 50, 100, 5, 70)
    (15, 100, 50, 100, 5, 70),

    (30, 70, 10, 50, 0, 40),
    (33, 57, 16, 34, -18, -2),
    (25, 65, 15, 45, -25, 5 ),
    (30, 70, 10, 50, -20, 20),
    (28, 68,8, 48,-28, 12 ),
    (
       25, 75,    # L亮度范围（典型值50±25）
       10, 60,    # a绿-红范围（典型值35±25）
       -30, 20    # b蓝-黄范围（典型值-5±25）
    ),
    (
       30, 70,    # L亮度范围（典型值50±20）
       15, 55,    # a绿-红范围（典型值35±20）
       -25, 15    # b蓝-黄范围（典型值-5±20）
    )

    # # 主阈值范围 (适用于大多数情况)
    # (20, 100, 60, 100, 10, 60),    # 高饱和度红色激光

    # # 辅助阈值范围 (适应不同条件)
    # (15, 100, 50, 100, 5, 70),     # 稍宽松范围
    # (10, 100, 40, 100, 0, 80)      # 最宽松范围
]

# 更宽松的激光颜色阈值范围 (LAB色彩空间)
# 格式: (L_min, L_max, A_min, A_max, B_min, B_max)
G_THRESHOLDS = [
    (98, 81, -48, 41, 14, 72)
]

# 激光点最小参数
MIN_PIXELS = 2      # 最小像素数
MIN_AREA = 15       # 最小区域
MERGE_BLOBS = True  # 合并相邻色块

def find_laser_point(img,THRESHOLDS):
    """
    在图像中寻找激光点
    返回: (x, y)坐标或(None, None)
    """
    # 尝试多个阈值范围
    for threshold in THRESHOLDS:
        blobs = img.find_blobs([threshold],
                              pixels_threshold=MIN_PIXELS,
                              area_threshold=MIN_AREA,
                              merge=MERGE_BLOBS)

        if blobs:
            # 找到面积最大的blob(最可能是激光点)
            largest_blob = max(blobs, key=lambda b: b.area())
            return (largest_blob.cx(), largest_blob.cy())

    return (None, None)

while True:
    clock.tick()
    img = sensor.snapshot()
    # 在IDE中使用工具查看实际LAB值
    # print(img.get_pixel(80, 60))  # 获取激光点位置的LAB值

    # 去除鱼眼畸变
    img.lens_corr(1.8)

    # 寻找激光点
    r_cx, r_cy= find_laser_point(img,R_THRESHOLDS)
    g_cx, g_cy = find_laser_point(img,G_THRESHOLDS)

    if r_cx is not None and r_cy is not None:
        # 绘制识别到的激光点
        img.draw_rectangle(int(r_cx)-5, int(r_cy)-5, 10, 10, color=(255, 0, 0), thickness=2)
        img.draw_cross(int(r_cx), int(r_cy), color=(0, 255, 0), thickness=1)

        # print("[%d*%d]" % (r_cx, r_cy))
    #     uart.write("[%d*%d]" % (r_cx, r_cy) + "\r\n")  # 添加换行符便于接收端解析
    # else:
    #     # 未检测到激光点
    #     # data = "[NULL]"
    #     print("[NULL]")
    #     uart.write("[NULL]"+ "\r\n")


    # 寻找激光点
    # g_cx, g_cy = find_laser_point(img,G_THRESHOLDS)

    if g_cx is not None and g_cy is not None:
        # 绘制识别到的激光点
        img.draw_rectangle(int(g_cx)-5, int(g_cy)-5, 10, 10, color=(0, 255, 0), thickness=2)
        img.draw_cross(int(g_cx), int(g_cy), color=(255, 0, 0), thickness=1)

        # print("[%d，%d]" % (g_cx, g_cy))
        # uart.write("[%d*%d]" % (g_cx, g_cy) + "\r\n")  # 添加换行符便于接收端解析
        # print("[%d*%d*%d*%d]" % (r_cx,r_cy,g_cx,g_cy))
        # uart.write("[%d*%d][%d,%d]" % (r_cx, r_cy,g_cx, g_cy) + "\r\n")  # 添加换行符便于接收端解析
    print("{%s*%s*%s*%s}" % (
        r_cx if r_cx is not None else "NULL",
        r_cy if r_cy is not None else "NULL",
        g_cx if g_cx is not None else "NULL",
        g_cy if g_cy is not None else "NULL"
    )+ "\r\n")
    uart.write("{%s*%s*%s*%s}" % (
    r_cx if r_cx is not None else "NULL",
    r_cy if r_cy is not None else "NULL",
    g_cx if g_cx is not None else "NULL",
    g_cy if g_cy is not None else "NULL"
    )+ "\r\n")
    # # 如果都未检测到
    # if r_cx is None and g_cx is None:
    #     print("[NULL]")
    #     uart.write("[NULL]"+ "\r\n")

    # 控制帧率，避免串口数据过载
    time.sleep_ms(50)

